/**
 *
 */
package jp.seraph.jsade.core;

import org.apache.commons.math.geometry.Vector3D;

import jp.seraph.jsade.perceptor.SeeObject;
import jp.seraph.jsade.perceptor.SeeObjectConfidence;
import jp.seraph.jsade.perceptor.SeeObjectType;
import jp.seraph.jsade.perceptor.SeePerceptor;

/**
 * とりあえず作る計算機
 * 確実に信用できるデータだけをとりあえず計算する。
 * 多分フィールド内にいたらフラグが三角形を生成する3つだけ見えている状態はない気がするので割愛
 */
public class DefaultSeeObjectCalculator extends AbstractSeeObjectCalculator {
    /**
     *
     */
    public DefaultSeeObjectCalculator(FieldContext aFieldContext) {
        mFieldContext = aFieldContext;
        initCoefficient();
    }

    private void initCoefficient() {

        double tFieldToCenter = mFieldContext.getWidth() / 2;
        double tGoalToCenter = mFieldContext.getGoalWidth() / 2;
        double tGoalToField = tFieldToCenter - tGoalToCenter;

        {
            mFieldGoalGoal = (tGoalToCenter + tFieldToCenter) / tGoalToField;
            mFieldGoalField = mFieldContext.getWidth() / tGoalToField;
            mGoalGoalField = (tGoalToCenter + tFieldToCenter) / mFieldContext.getGoalWidth();

        }
    }

    private FieldContext mFieldContext;

    private double mFieldGoalGoal;

    private double mFieldGoalField;

    private double mGoalGoalField;

    /**
     * @see jp.seraph.jsade.core.AbstractSeeObjectCalculator#calcLeftField1(jp.seraph.jsade.perceptor.SeePerceptor)
     */
    @Override
    protected SeeObject calcLeftField1(SeePerceptor aSeePerceptor) {
        SeePerceptor tSeePerceptor = aSeePerceptor;
        SeeObject tLF2 = tSeePerceptor.getLeftField2();
        SeeObject tLG1 = tSeePerceptor.getLeftGoal1();
        SeeObject tLG2 = tSeePerceptor.getLeftGoal2();

        if (tLF2 != null && tLG2 != null) {
            Vector3D tLf2Lf1 = tLG2.getVector().subtract(tLF2.getVector()).scalarMultiply(mFieldGoalField);
            return new SeeObject(SeeObjectType.LEFT_FIELD_1, SeeObjectConfidence.Calculation, tLF2.getVector().add(tLf2Lf1));
        } else if (tLG1 != null && tLG2 != null) {
            Vector3D tLg2Lf1 = tLG1.getVector().subtract(tLG2.getVector()).scalarMultiply(mGoalGoalField);
            return new SeeObject(SeeObjectType.LEFT_FIELD_1, SeeObjectConfidence.Calculation, tLG2.getVector().add(tLg2Lf1));
        } else {
            return new SeeObject(SeeObjectType.LEFT_FIELD_1, SeeObjectConfidence.Unpredictable, 0, 0, 0);
        }
    }

    /**
     * @see jp.seraph.jsade.core.AbstractSeeObjectCalculator#calcLeftField2(jp.seraph.jsade.perceptor.SeePerceptor)
     */
    @Override
    protected SeeObject calcLeftField2(SeePerceptor aSeePerceptor) {
        SeePerceptor tSeePerceptor = aSeePerceptor;
        SeeObject tLF1 = tSeePerceptor.getLeftField1();
        SeeObject tLG2 = tSeePerceptor.getLeftGoal2();
        SeeObject tLG1 = tSeePerceptor.getLeftGoal1();

        if (tLF1 != null && tLG1 != null) {
            Vector3D tLf1Lf2 = tLG1.getVector().subtract(tLF1.getVector()).scalarMultiply(mFieldGoalField);
            return new SeeObject(SeeObjectType.LEFT_FIELD_2, SeeObjectConfidence.Calculation, tLF1.getVector().add(tLf1Lf2));
        } else if (tLG2 != null && tLG1 != null) {
            Vector3D tLg1Lf2 = tLG2.getVector().subtract(tLG1.getVector()).scalarMultiply(mGoalGoalField);
            return new SeeObject(SeeObjectType.LEFT_FIELD_2, SeeObjectConfidence.Calculation, tLG1.getVector().add(tLg1Lf2));
        } else {
            return new SeeObject(SeeObjectType.LEFT_FIELD_2, SeeObjectConfidence.Unpredictable, 0, 0, 0);
        }
    }

    /**
     * @see jp.seraph.jsade.core.AbstractSeeObjectCalculator#calcLeftGoal1(jp.seraph.jsade.perceptor.SeePerceptor)
     */
    @Override
    protected SeeObject calcLeftGoal1(SeePerceptor aSeePerceptor) {
        SeePerceptor tSeePerceptor = aSeePerceptor;
        SeeObject tLG2 = tSeePerceptor.getLeftGoal2();
        SeeObject tLF2 = tSeePerceptor.getLeftField2();

        if(tLG2 != null && tLF2 != null){
            Vector3D tLf2Lg1 = tLG2.getVector().subtract(tLF2.getVector()).scalarMultiply(mFieldGoalGoal);
            return new SeeObject(SeeObjectType.LEFT_GOAL_1, SeeObjectConfidence.Calculation, tLF2.getVector().add(tLf2Lg1));
        }else{
            return new SeeObject(SeeObjectType.LEFT_GOAL_1, SeeObjectConfidence.Unpredictable, 0, 0, 0);
        }
    }

    /**
     * @see jp.seraph.jsade.core.AbstractSeeObjectCalculator#calcLeftGoal2(jp.seraph.jsade.perceptor.SeePerceptor)
     */
    @Override
    protected SeeObject calcLeftGoal2(SeePerceptor aSeePerceptor) {
        SeePerceptor tSeePerceptor = aSeePerceptor;
        SeeObject tLG1 = tSeePerceptor.getLeftGoal1();
        SeeObject tLF1 = tSeePerceptor.getLeftField1();

        if(tLG1 != null && tLF1 != null){
            Vector3D tLf1Lg2 = tLG1.getVector().subtract(tLF1.getVector()).scalarMultiply(mFieldGoalGoal);
            return new SeeObject(SeeObjectType.LEFT_GOAL_1, SeeObjectConfidence.Calculation, tLF1.getVector().add(tLf1Lg2));
        }else{
            return new SeeObject(SeeObjectType.LEFT_GOAL_1, SeeObjectConfidence.Unpredictable, 0, 0, 0);
        }
    }

    /**
     * @see jp.seraph.jsade.core.AbstractSeeObjectCalculator#calcRightField1(jp.seraph.jsade.perceptor.SeePerceptor)
     */
    @Override
    protected SeeObject calcRightField1(SeePerceptor aSeePerceptor) {
        SeePerceptor tSeePerceptor = aSeePerceptor;
        SeeObject tRF2 = tSeePerceptor.getRightField2();
        SeeObject tRG1 = tSeePerceptor.getRightGoal1();
        SeeObject tRG2 = tSeePerceptor.getRightGoal2();

        if (tRF2 != null && tRG2 != null) {
            Vector3D tRf2Rf1 = tRG2.getVector().subtract(tRF2.getVector()).scalarMultiply(mFieldGoalField);
            return new SeeObject(SeeObjectType.RIGHT_FIELD_1, SeeObjectConfidence.Calculation, tRF2.getVector().add(tRf2Rf1));
        } else if (tRG1 != null && tRG2 != null) {
            Vector3D tRg2Rf1 = tRG1.getVector().subtract(tRG2.getVector()).scalarMultiply(mGoalGoalField);
            return new SeeObject(SeeObjectType.RIGHT_FIELD_1, SeeObjectConfidence.Calculation, tRG2.getVector().add(tRg2Rf1));
        } else {
            return new SeeObject(SeeObjectType.RIGHT_FIELD_1, SeeObjectConfidence.Unpredictable, 0, 0, 0);
        }
    }

    /**
     * @see jp.seraph.jsade.core.AbstractSeeObjectCalculator#calcRightField2(jp.seraph.jsade.perceptor.SeePerceptor)
     */
    @Override
    protected SeeObject calcRightField2(SeePerceptor aSeePerceptor) {
        SeePerceptor tSeePerceptor = aSeePerceptor;
        SeeObject tRF1 = tSeePerceptor.getRightField1();
        SeeObject tRG2 = tSeePerceptor.getRightGoal2();
        SeeObject tRG1 = tSeePerceptor.getRightGoal1();

        if (tRF1 != null && tRG1 != null) {
            Vector3D tRf1Rf2 = tRG1.getVector().subtract(tRF1.getVector()).scalarMultiply(mFieldGoalField);
            return new SeeObject(SeeObjectType.RIGHT_FIELD_2, SeeObjectConfidence.Calculation, tRF1.getVector().add(tRf1Rf2));
        } else if (tRG2 != null && tRG1 != null) {
            Vector3D tRg1Rf2 = tRG2.getVector().subtract(tRG1.getVector()).scalarMultiply(mGoalGoalField);
            return new SeeObject(SeeObjectType.RIGHT_FIELD_2, SeeObjectConfidence.Calculation, tRG1.getVector().add(tRg1Rf2));
        } else {
            return new SeeObject(SeeObjectType.RIGHT_FIELD_2, SeeObjectConfidence.Unpredictable, 0, 0, 0);
        }
    }

    /**
     * @see jp.seraph.jsade.core.AbstractSeeObjectCalculator#calcRightGoal1(jp.seraph.jsade.perceptor.SeePerceptor)
     */
    @Override
    protected SeeObject calcRightGoal1(SeePerceptor aSeePerceptor) {
        SeePerceptor tSeePerceptor = aSeePerceptor;
        SeeObject tRG2 = tSeePerceptor.getRightGoal2();
        SeeObject tRF2 = tSeePerceptor.getRightField2();

        if(tRG2 != null && tRF2 != null){
            Vector3D tRf2Rg1 = tRG2.getVector().subtract(tRF2.getVector()).scalarMultiply(mFieldGoalGoal);
            return new SeeObject(SeeObjectType.RIGHT_GOAL_1, SeeObjectConfidence.Calculation, tRF2.getVector().add(tRf2Rg1));
        }else{
            return new SeeObject(SeeObjectType.RIGHT_GOAL_1, SeeObjectConfidence.Unpredictable, 0, 0, 0);
        }
    }

    /**
     * @see jp.seraph.jsade.core.AbstractSeeObjectCalculator#calcRightGoal2(jp.seraph.jsade.perceptor.SeePerceptor)
     */
    @Override
    protected SeeObject calcRightGoal2(SeePerceptor aSeePerceptor) {
        SeePerceptor tSeePerceptor = aSeePerceptor;
        SeeObject tRG1 = tSeePerceptor.getRightGoal1();
        SeeObject tRF1 = tSeePerceptor.getRightField1();

        if(tRG1 != null && tRF1 != null){
            Vector3D tRf1Rg2 = tRG1.getVector().subtract(tRF1.getVector()).scalarMultiply(mFieldGoalGoal);
            return new SeeObject(SeeObjectType.RIGHT_GOAL_2, SeeObjectConfidence.Calculation, tRF1.getVector().add(tRf1Rg2));
        }else{
            return new SeeObject(SeeObjectType.RIGHT_GOAL_2, SeeObjectConfidence.Unpredictable, 0, 0, 0);
        }
    }

}
